// Lindsay Gilmour
// Adam Lusch
// Dan Niceswanger
//
// Computer Graphics Project 2:
// Teapot racing!

#include "Teapot.h"

#include <iostream>
using namespace std;

#include <cmath>

// Static variables and functions

GLuint Teapot::baseList;
GLuint Teapot::simpleList;

const GLdouble Teapot::STEERING_MIN = -40.0;
const GLdouble Teapot::STEERING_MAX = 40.0;
const GLdouble Teapot::STEERING_INCREMENT = 4.0;

const GLdouble Teapot::TIRE_FRONT_SPEED = 4.0;
const GLdouble Teapot::TIRE_REAR_SPEED = 5.333;

const GLdouble Teapot::NORMAL_ACCEL_AMOUNT = 60.0;
const GLdouble Teapot::NORMAL_BRAKE_AMOUNT = 100.0;

void Teapot::setup()
{
	Tire::setup();

	baseList = glGenLists(1);
	
	glNewList(baseList, GL_COMPILE);
	
		glPushMatrix();
		glPushAttrib(GL_ALL_ATTRIB_BITS);
	
			// Shiny metal
			GLfloat spec[] = {1.0, 1.0, 1.0, 1.0};
			glMaterialfv(GL_FRONT, GL_SPECULAR, spec);
			glMaterialf(GL_FRONT, GL_SHININESS, 32);
	
			glRotatef(90, 0.0, 1.0, 0.0);
			// For now, at least
			glDisable(GL_TEXTURE_2D);
			glFrontFace(GL_CW);
			mySolidTeapot(6.0, 5);
			glFrontFace(GL_CCW);
		
		glPopAttrib();
		glPopMatrix();
	
	glEndList();
	
	simpleList = glGenLists(1);
	
	glNewList(simpleList, GL_COMPILE);
	
		glutSolidSphere(6.0, 10, 10);
	
	glEndList();
}

void Teapot::cleanup()
{
	glDeleteLists(baseList, 1);
	
	Tire::cleanup();
}

// Instance methods

Teapot::Teapot(b2World* w, GLdouble x, GLdouble z, GLdouble h)
{
	world = w;
	
	turnAngle = 0;
	color = new GLfloat[4];
	
	setColor(1.0, 1.0, 1.0, 1.0);
	
	accelAmount = NORMAL_ACCEL_AMOUNT;
	brakeAmount = NORMAL_BRAKE_AMOUNT;
	
	// TODO: Better collision shape?
	
	b2BodyDef bodyDef;
	bodyDef.position.Set(x, z);
	bodyDef.angle = h * M_PI / 180.0;
	bodyDef.linearDamping = 0.5;
	bodyDef.angularDamping = 50.0;
	body = world->CreateBody(&bodyDef);

	b2PolygonDef squareDef;
	squareDef.vertices[0] = b2Vec2(0.28, 0.22);
	squareDef.vertices[1] = b2Vec2(-0.28, 0.22);
	squareDef.vertices[2] = b2Vec2(-0.28, -0.22);
	squareDef.vertices[3] = b2Vec2(0.28, -0.22);
	
	squareDef.vertexCount = 4;
	squareDef.density = 50.0f;
	squareDef.friction = 0.0f;
	squareDef.restitution = 0.0;
	squareDef.userData = this;
	body->CreateShape(&squareDef);

	body->SetMassFromShapes();
}

Teapot::Teapot(const Teapot& t)
{
	color = t.color;
	
	flTire = t.flTire;
	frTire = t.frTire;
	rlTire = t.rlTire;
	rrTire = t.rrTire;
	
	turnAngle = t.turnAngle;
	
	accelAmount = t.accelAmount;
	brakeAmount = t.brakeAmount;
	coastAmount = t.coastAmount;
	
	dir = t.dir;
	accel = t.accel;
	
	body = t.body;
	world = t.world;
}

Teapot::Teapot()
{
	color = new GLfloat[4];
	
	setColor(1.0, 1.0, 1.0, 1.0);
	
	accelAmount = NORMAL_ACCEL_AMOUNT;
	brakeAmount = NORMAL_BRAKE_AMOUNT;
}

Teapot::~Teapot()
{
	delete[] color;
}

void Teapot::update()
{

	if (dir == LEFT && turnAngle < STEERING_MAX || dir == STRAIGHT && turnAngle < 0.0)
	{
		turnAngle += STEERING_INCREMENT;
	}
	else if (dir == RIGHT && turnAngle > STEERING_MIN || dir == STRAIGHT && turnAngle > 0.0)
	{
		turnAngle -= STEERING_INCREMENT;
	}
	
	b2Shape* shapes = body->GetShapeList();
	b2AABB box;
	shapes[0].ComputeAABB(&box, body->GetXForm());
	
	static const int32 k_maxCount = 10;
	b2Shape* overlapping[k_maxCount];
	
	int32 numShapes = world->Query(box, overlapping, k_maxCount);
	
	b2XForm ident;
	ident.SetIdentity();
	
	b2Vec2 pos = body->GetPosition();
	GLdouble angle = body->GetAngle();
	
	b2Mat22 rotate(-angle);
	
	b2Vec2 pos1 = pos;
	pos1 += rotate.Solve(b2Vec2(0.28, 0.22));
	b2Vec2 pos2 = pos;
	pos2 += rotate.Solve(b2Vec2(-0.28, 0.22));
	b2Vec2 pos3 = pos;
	pos3 += rotate.Solve(b2Vec2(-0.28, -0.22));
	b2Vec2 pos4 = pos;
	pos4 += rotate.Solve(b2Vec2(0.28, -0.22));
	
	
	int cornersOnRoad = 0;
	
	for(int i = 0; i < numShapes; i++)
	{
		if (overlapping[i]->GetUserData() == (void*)1337)
		{
			cornersOnRoad += overlapping[i]->TestPoint(ident, pos1);
			cornersOnRoad += overlapping[i]->TestPoint(ident, pos2);
			cornersOnRoad += overlapping[i]->TestPoint(ident, pos3);
			cornersOnRoad += overlapping[i]->TestPoint(ident, pos4);
		}
	}

	b2Vec2 vel = body->GetLinearVelocity();
	GLdouble speed = vel.Length();

	b2Vec2 center = body->GetWorldPoint(b2Vec2(0.0, 0.0));
	
	if (cornersOnRoad < 3)
	{
		b2Vec2 slowDown = -vel;
		slowDown *= 10;
		body->ApplyForce(slowDown, center);
	}
	
	b2Vec2 currentDir = b2Vec2(speed * cos(angle), speed * sin(angle));
	GLdouble dirVelDot = b2Dot(currentDir, vel);
	
	GLdouble turnAmount;
	
	turnAmount = (turnAngle * M_PI / 180.0) / 32.0 * min(speed * speed, 0.95);
	
	// Forward
	if (dirVelDot > 0)
		angle -= turnAmount;
	// Reverse
	else
		angle += turnAmount;

	// Update angle
	body->SetXForm(body->GetPosition(), angle);
	
	b2Vec2 accelForce(accelAmount * cos(angle), accelAmount * sin(angle));
	b2Vec2 brakeForce(brakeAmount * cos(angle), brakeAmount * sin(angle));
	
	switch(accel)
	{
		case FORWARD:
			if (dirVelDot > 0)
			{
				body->ApplyForce(accelForce, center);
			}
			else
			{
				body->ApplyForce(brakeForce, center);
			}
			break;
		
		case REVERSE:
			if (dirVelDot > 0)
			{
				body->ApplyForce(-brakeForce, center);
			}
			else
			{
				body->ApplyForce(-accelForce, center);
			}
			break;
		
		case COAST:
			break;
	}
	
	if (speed > 0)
	{
		currentDir = b2Vec2(speed * cos(angle), speed * sin(angle));
		dirVelDot = b2Dot(currentDir, vel);
		
		// Project velocity onto direction
		b2Vec2 proj = dirVelDot / currentDir.LengthSquared() * currentDir;
		b2Vec2 damp = proj - vel;
		damp *= 200; // TODO: Handle this better
	
		body->ApplyForce(damp, center);
	}
	
	GLdouble rotateModifier;
	if (dirVelDot > 0)
	{
		rotateModifier = min(speed, 1.5);
	}
	else
	{
		rotateModifier = max(-speed, -1.5);
	}
	
	rrTire.rotate(-TIRE_REAR_SPEED * rotateModifier);
	rlTire.rotate(-TIRE_REAR_SPEED * rotateModifier);
	frTire.rotate(-TIRE_FRONT_SPEED * rotateModifier);
	flTire.rotate(-TIRE_FRONT_SPEED * rotateModifier);
}

void Teapot::drawSimple(GLfloat scale) const
{
	b2Vec2 pos = body->GetPosition();

	glPushMatrix();
		glTranslatef(pos.x, 0.0, pos.y);
		glRotatef(-body->GetAngle() * 180.0 / M_PI, 0.0, 1.0, 0.0);
		glTranslatef(0.0, 0.0, 0.0);
		glScalef(scale, scale, scale);
		glScalef(0.03, 0.03, 0.03);
		
		glColor4fv(color);
		glCallList(simpleList);
		
	glPopMatrix();
}

void Teapot::draw(GLfloat scale) const
{
	b2Vec2 pos = body->GetPosition();

	glPushMatrix();
		glTranslatef(pos.x, 0.0, pos.y);
		glRotatef(-body->GetAngle() * 180.0 / M_PI, 0.0, 1.0, 0.0);
		glTranslatef(0.0, 0.0, 0.0);
		glScalef(scale, scale, scale);
		glScalef(0.03, 0.03, 0.03);

		glPushMatrix();
			glColor4fv(color);
			glCallList(baseList);
		
			glPushMatrix();
				glTranslatef(7.0, -4.0, 5.5);
				glScalef(0.75, 0.75, 0.75);
				glRotatef(turnAngle, 0.0, 1.0, 0.0);
				frTire.draw();
			glPopMatrix();
		
			glPushMatrix();
				glTranslatef(7.0, -4.0, -5.5);
				glScalef(0.75, 0.75, -0.75);
				glRotatef(-turnAngle, 0.0, 1.0, 0.0);
				flTire.draw();
			glPopMatrix();
		
			glPushMatrix();
				glTranslatef(-6.0, -3.5, 5.0);
				rrTire.draw();
			glPopMatrix();
		
			glPushMatrix();
				glTranslatef(-6.0, -3.5, -5.0);
				glScalef(1.0, 1.0, -1.0);
				rlTire.draw();
			glPopMatrix();
		glPopMatrix();
	
	glPopMatrix();
}

GLdouble Teapot::getX() const
{
	return body->GetPosition().x;
}

GLdouble Teapot::getZ() const
{
	return body->GetPosition().y;
}

GLdouble Teapot::getHeading() const
{
	return body->GetAngle();
}

GLdouble Teapot::getSpeed() const
{
	b2Vec2 vel = body->GetLinearVelocity();
	return vel.Length();
}

void Teapot::turn(Teapot::Direction d)
{
	dir = d;
}

void Teapot::accelerate(Teapot::Acceleration a)
{
	accel = a;
}

void Teapot::setColor(GLfloat r, GLfloat g, GLfloat b, GLfloat a)
{
	color[0] = r;
	color[1] = g;
	color[2] = b;
	color[3] = a;
}

